#!/usr/bin/env python3
"""
ROS2伺服电机控制节点 - 改进的断电保护位置记忆功能
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32, Int32, Bool
import serial
import struct
import time
import threading
import json
import os
import atexit
from pathlib import Path
from enum import Enum
import signal
import sys

class LimitStatus(Enum):
    NORMAL = 0
    AT_MIN_LIMIT = 1
    AT_MAX_LIMIT = 2
    EXCEEDED_MIN_LIMIT = 3
    EXCEEDED_MAX_LIMIT = 4

class AIMotorControl(Node):
    def __init__(self):
        super().__init__('aimotor_control')
        
        # 参数声明
        self.declare_parameter('port', '/dev/ttyUSB0')
        self.declare_parameter('baudrate', 57600)
        self.declare_parameter('slave_id', 1)
        self.declare_parameter('pulses_per_revolution', 1000)
        self.declare_parameter('position_query_interval', 0.2)
        self.declare_parameter('position_save_interval', 1.0)
        self.declare_parameter('position_change_threshold', 50.0)
        self.declare_parameter('soft_limit_min', -350000.0)
        self.declare_parameter('soft_limit_max', 350000.0)
        self.declare_parameter('limit_tolerance', 10.0)
        self.declare_parameter('emergency_stop_distance', 1000.0)
        self.declare_parameter('position_file', 'motor_position.json')
        self.declare_parameter('enable_absolute_positioning', True)  # 新增：启用绝对位置模式
        
        # 获取参数
        port = self.get_parameter('port').value
        baudrate = self.get_parameter('baudrate').value
        self.slave_id = self.get_parameter('slave_id').value
        self.pulses_per_rev = self.get_parameter('pulses_per_revolution').value
        query_interval = self.get_parameter('position_query_interval').value
        self.save_interval = self.get_parameter('position_save_interval').value
        self.position_change_threshold = self.get_parameter('position_change_threshold').value
        self.soft_limit_min = self.get_parameter('soft_limit_min').value
        self.soft_limit_max = self.get_parameter('soft_limit_max').value
        self.limit_tolerance = self.get_parameter('limit_tolerance').value
        self.emergency_stop_distance = self.get_parameter('emergency_stop_distance').value
        self.position_file = self.get_parameter('position_file').value
        self.enable_absolute_positioning = self.get_parameter('enable_absolute_positioning').value
        
        # 电机连接
        self.ser = self.connect_motor(port, baudrate)
        if not self.ser:
            self.get_logger().error('电机连接失败，节点将退出')
            return
        
        # 线程锁
        self.ser_lock = threading.Lock()
        self.position_lock = threading.Lock()
        self.file_lock = threading.Lock()
        
        # 位置管理变量 - 关键改进
        self.last_hardware_position = 0
        self.software_position_offset = 0
        self.current_software_position = 0
        self.last_saved_position = 0
        self.position_changed = False
        self.unsaved_changes = False
        
        # 新增：断电恢复状态管理
        self.power_loss_recovered = False
        self.initial_hardware_position = 0
        
        # 限位状态管理
        self.limit_status = LimitStatus.NORMAL
        self.is_moving = False
        self.current_move_target = 0
        self.emergency_stop_triggered = False
        
        # 注册退出处理
        self.register_exit_handlers()
        
        # 关键改进：先读取当前硬件位置，再加载保存的位置
        self.initial_hardware_position = self.read_current_position()
        if self.initial_hardware_position is None:
            self.get_logger().error('无法读取电机硬件位置，使用默认值0')
            self.initial_hardware_position = 0
        
        self.get_logger().info(f'初始硬件位置: {self.initial_hardware_position}')
        
        # 加载保存的位置
        self.load_saved_position()
        
        # 关键改进：计算位置偏移量
        self.calculate_position_offset()
        
        self.last_saved_position = self.current_software_position
        
        # 当前控制模式
        self.current_mode = None
        
        # 订阅者
        self.velocity_sub = self.create_subscription(
            Float32,
            'motor_velocity',
            self.velocity_callback,
            10
        )
        
        self.position_sub = self.create_subscription(
            Float32,
            'motor_position',
            self.position_callback,
            10
        )
        
        self.circle_sub = self.create_subscription(
            Float32,
            'motor_circle',
            self.circle_callback,
            10
        )
        
        self.control_mode_sub = self.create_subscription(
            Int32,
            'motor_control_mode',
            self.control_mode_callback,
            10
        )
        
        self.emergency_stop_sub = self.create_subscription(
            Bool,
            'emergency_stop',
            self.emergency_stop_callback,
            10
        )
        
        # 新增：软件位置设置服务
        self.set_software_position_sub = self.create_subscription(
            Float32,
            'set_software_position',
            self.set_software_position_callback,
            10
        )
        
        # 发布器
        self.software_position_pub = self.create_publisher(
            Float32,
            'motor_software_position',
            10
        )
        
        self.limit_status_pub = self.create_publisher(
            Int32,
            'motor_limit_status',
            10
        )
        
        self.moving_status_pub = self.create_publisher(
            Bool,
            'motor_moving_status',
            10
        )
        
        # 新增：断电恢复状态发布
        self.power_recovery_pub = self.create_publisher(
            Bool,
            'motor_power_recovered',
            10
        )
        
        # 定时器
        self.position_timer = self.create_timer(
            query_interval, 
            self.position_query_callback
        )
        
        self.save_timer = self.create_timer(
            self.save_interval,
            self.save_position_callback
        )
        
        self.limit_check_timer = self.create_timer(
            0.1,
            self.limit_check_callback
        )
        
        # 强制保存计数器
        self.force_save_counter = 0
        self.force_save_interval = 10
        
        self.get_logger().info('AIMotor控制节点已启动(改进的断电保护)')
        self.get_logger().info(f'软限位范围: [{self.soft_limit_min}, {self.soft_limit_max}]')
        self.get_logger().info(f'初始软件位置: {self.current_software_position}')
        self.get_logger().info(f'软件位置偏移量: {self.software_position_offset}')
        
    def calculate_position_offset(self):
        """计算位置偏移量 - 关键改进"""
        if self.enable_absolute_positioning:
            # 绝对位置模式：软件位置 = 硬件位置 + 偏移量
            self.software_position_offset = self.current_software_position - self.initial_hardware_position
            self.get_logger().info(f'计算位置偏移量: {self.software_position_offset}')
        else:
            # 相对位置模式（原有逻辑）
            self.software_position_offset = self.current_software_position - self.last_hardware_position
            self.last_hardware_position = self.initial_hardware_position
            
    def update_software_position(self, hardware_position):
        """更新软件位置 - 关键改进"""
        with self.position_lock:
            if self.enable_absolute_positioning:
                # 绝对位置模式：直接计算
                new_software_position = hardware_position + self.software_position_offset
                position_diff = new_software_position - self.current_software_position
            else:
                # 相对位置模式（原有逻辑）
                position_diff = hardware_position - self.last_hardware_position
                if position_diff > 0x7FFFFFFF:
                    position_diff -= 0x100000000
                elif position_diff < -0x7FFFFFFF:
                    position_diff += 0x100000000
                new_software_position = self.current_software_position + position_diff
                self.last_hardware_position = hardware_position
            
            self.current_software_position = new_software_position
            
            # 标记有未保存的更改
            if abs(position_diff) > 0:
                self.unsaved_changes = True
            
            return self.current_software_position

    def load_saved_position(self):
        """从文件加载保存的位置 - 关键改进"""
        try:
            position_file = Path(self.position_file)
            if position_file.exists():
                with open(position_file, 'r') as f:
                    data = json.load(f)
                    
                    # 兼容新旧版本数据格式
                    if 'absolute_position' in data:
                        # 新格式：使用绝对位置
                        self.current_software_position = data.get('absolute_position', 0)
                        self.get_logger().info(f'从文件加载绝对位置: {self.current_software_position}')
                    else:
                        # 旧格式：兼容处理
                        self.software_position_offset = data.get('software_position_offset', 0)
                        self.current_software_position = data.get('current_software_position', 0)
                        self.last_hardware_position = data.get('last_hardware_position', 0)
                        self.get_logger().info(f'从文件加载兼容位置: 软件位置={self.current_software_position}')
                    
                    # 检查断电恢复情况
                    saved_hardware_pos = data.get('last_hardware_position', 0)
                    hardware_diff = abs(self.initial_hardware_position - saved_hardware_pos)
                    
                    if hardware_diff > self.pulses_per_rev:  # 如果硬件位置变化超过1圈
                        self.get_logger().warn(f'检测到可能的断电情况: 硬件位置变化 {hardware_diff}')
                        self.power_loss_recovered = True
                        
                        # 发布断电恢复状态
                        recovery_msg = Bool()
                        recovery_msg.data = True
                        self.power_recovery_pub.publish(recovery_msg)
                
                self.get_logger().info(f'位置文件加载成功: 软件位置={self.current_software_position}')
            else:
                self.get_logger().info('未找到位置保存文件，使用默认位置0')
                self.current_software_position = 0
                
        except Exception as e:
            self.get_logger().error(f'加载保存位置失败: {e}')
            self.current_software_position = 0

    def save_current_position(self, force=False):
        """保存当前位置到文件 - 关键改进"""
        try:
            with self.position_lock:
                current_pos = self.current_software_position
                current_hardware_pos = self.last_hardware_position
                
                # 检查位置变化是否超过阈值
                position_changed = abs(current_pos - self.last_saved_position) > self.position_change_threshold
                
                # 如果强制保存或位置变化超过阈值，则保存
                if force or position_changed or self.unsaved_changes:
                    # 新格式：保存绝对位置和硬件位置
                    data = {
                        'absolute_position': current_pos,  # 关键：保存绝对位置
                        'last_hardware_position': current_hardware_pos,
                        'software_position_offset': self.software_position_offset,
                        'save_timestamp': time.time(),
                        'soft_limit_min': self.soft_limit_min,
                        'soft_limit_max': self.soft_limit_max,
                        'version': '2.0'  # 版本标识
                    }
                    
                    with self.file_lock:
                        # 确保目录存在
                        os.makedirs(os.path.dirname(os.path.abspath(self.position_file)), exist_ok=True)
                        
                        with open(self.position_file, 'w') as f:
                            json.dump(data, f, indent=2)
                        
                        self.last_saved_position = current_pos
                        self.unsaved_changes = False
                    
                    if force:
                        self.get_logger().info(f'强制保存位置: {current_pos}')
                    else:
                        self.get_logger().debug(f'位置已保存: {current_pos}')
                
        except Exception as e:
            self.get_logger().error(f'保存位置失败: {e}')

    def set_software_position_callback(self, msg):
        """设置软件位置回调"""
        new_position = msg.data
        self.set_software_position(new_position)

    def set_software_position(self, new_position):
        """设置软件位置 - 关键改进"""
        with self.position_lock:
            old_position = self.current_software_position
            self.current_software_position = float(new_position)
            
            # 更新偏移量以保持硬件位置不变
            current_hardware_pos = self.read_current_position()
            if current_hardware_pos is not None:
                self.software_position_offset = self.current_software_position - current_hardware_pos
                self.last_hardware_position = current_hardware_pos
            
            self.unsaved_changes = True
            self.get_logger().info(f'软件位置已设置: {old_position} -> {new_position}')
            self.save_current_position(force=True)

    # 以下函数保持不变（为保持完整性包含，实际无需修改）
    def register_exit_handlers(self):
        """注册退出信号处理器"""
        atexit.register(self.emergency_save)
        
        def signal_handler(sig, frame):
            self.get_logger().info(f'接收到信号 {sig}，正在保存位置...')
            self.emergency_save()
            sys.exit(0)
            
        signal.signal(signal.SIGINT, signal_handler)
        signal.signal(signal.SIGTERM, signal_handler)
        
    def emergency_save(self):
        """紧急保存位置"""
        try:
            self.get_logger().info("执行紧急位置保存...")
            self.save_current_position(force=True)
            self.get_logger().info("紧急位置保存完成")
        except Exception as e:
            self.get_logger().error(f"紧急位置保存失败: {e}")
    
    def check_limits(self, position=None):
        """检查限位状态"""
        if position is None:
            position = self.current_software_position
        
        if position <= self.soft_limit_min:
            if position < self.soft_limit_min - self.limit_tolerance:
                return LimitStatus.EXCEEDED_MIN_LIMIT
            else:
                return LimitStatus.AT_MIN_LIMIT
        elif position >= self.soft_limit_max:
            if position > self.soft_limit_max + self.limit_tolerance:
                return LimitStatus.EXCEEDED_MAX_LIMIT
            else:
                return LimitStatus.AT_MAX_LIMIT
        else:
            return LimitStatus.NORMAL
    
    def limit_check_callback(self):
        """限位检查定时器回调"""
        new_limit_status = self.check_limits()
        
        if new_limit_status != self.limit_status:
            self.limit_status = new_limit_status
            
            status_msg = Int32()
            status_msg.data = self.limit_status.value
            self.limit_status_pub.publish(status_msg)
            
            if self.limit_status in [LimitStatus.EXCEEDED_MIN_LIMIT, LimitStatus.EXCEEDED_MAX_LIMIT]:
                self.get_logger().error(f'超出软限位! 当前位置: {self.current_software_position}, 状态: {self.limit_status}')
                self.emergency_stop()
            elif self.limit_status in [LimitStatus.AT_MIN_LIMIT, LimitStatus.AT_MAX_LIMIT]:
                self.get_logger().warn(f'接近软限位! 当前位置: {self.current_software_position}, 状态: {self.limit_status}')
                if self.is_moving:
                    self.safe_stop()
    
    def emergency_stop(self):
        """紧急停止"""
        self.get_logger().error('执行紧急停止!')
        self.emergency_stop_triggered = True
        self.stop_motor()
        
        moving_msg = Bool()
        moving_msg.data = False
        self.moving_status_pub.publish(moving_msg)
        
        self.is_moving = False
        self.save_current_position(force=True)
    
    def safe_stop(self):
        """安全停止"""
        self.get_logger().warn('安全停止: 接近限位边界')
        self.stop_motor()
        self.is_moving = False
        
        moving_msg = Bool()
        moving_msg.data = False
        self.moving_status_pub.publish(moving_msg)
        self.save_current_position(force=True)
    
    def can_move_to_position(self, target_position):
        """检查是否可以移动到目标位置"""
        if self.emergency_stop_triggered:
            return False, "紧急停止状态中"
        
        current_status = self.check_limits()
        target_status = self.check_limits(target_position)
        
        if current_status in [LimitStatus.EXCEEDED_MIN_LIMIT, LimitStatus.EXCEEDED_MAX_LIMIT]:
            return False, f"当前位置已超出限位: {current_status}"
        
        if target_status in [LimitStatus.EXCEEDED_MIN_LIMIT, LimitStatus.EXCEEDED_MAX_LIMIT]:
            return False, f"目标位置超出限位: {target_status}"
        
        if target_position > self.current_software_position:
            if current_status == LimitStatus.AT_MAX_LIMIT:
                return False, "已达正向限位，无法继续正向移动"
        else:
            if current_status == LimitStatus.AT_MIN_LIMIT:
                return False, "已达负向限位，无法继续负向移动"
        
        return True, "可以移动"
    
    def save_position_callback(self):
        """定时保存位置"""
        self.save_current_position()
        
        if self.unsaved_changes:
            self.save_current_position(force=True)
    
    def calculate_crc(self, data):
        """计算Modbus CRC16校验码"""
        crc = 0xFFFF
        for byte in data:
            crc ^= byte
            for _ in range(8):
                if crc & 0x0001:
                    crc >>= 1
                    crc ^= 0xA001
                else:
                    crc >>= 1
        return crc.to_bytes(2, byteorder='little')
    
    def connect_motor(self, port, baudrate):
        """连接电机"""
        try:
            ser = serial.Serial(
                port=port,
                baudrate=baudrate,
                bytesize=8,
                parity='N',
                stopbits=1,
                timeout=1.0
            )
            time.sleep(2)
            self.get_logger().info(f'电机连接成功: {port}')
            return ser
        except Exception as e:
            self.get_logger().error(f'电机连接失败: {e}')
            return None

    def read_current_position(self):
        """读取电机当前位置"""
        try:
            with self.ser_lock:
                read_cmd = bytearray([
                    self.slave_id, 0x03, 0x0B, 0x07, 0x00, 0x02
                ])
                crc = self.calculate_crc(read_cmd)
                read_cmd.extend(crc)
                self.ser.write(read_cmd)
                
                response = self.ser.read(9)
                if len(response) < 9:
                    return None
                
                if response[0] != self.slave_id or response[1] != 0x03:
                    return None
                
                low_word = (response[3] << 8) | response[4]
                high_word = (response[5] << 8) | response[6]
                raw_position = (high_word << 16) | low_word
                
                if raw_position >= 0x80000000:
                    position = raw_position - 0x100000000
                else:
                    position = raw_position
                    
                return position
                
        except Exception as e:
            self.get_logger().error(f'读取位置失败: {e}')
            return None

    def position_query_callback(self):
        """位置查询定时器回调"""
        hardware_position = self.read_current_position()
        
        if hardware_position is not None:
            software_position = self.update_software_position(hardware_position)
            
            # 发布软件位置
            position_msg = Float32()
            position_msg.data = float(software_position)
            self.software_position_pub.publish(position_msg)
            
            # 强制保存机制
            self.force_save_counter += 1
            if self.force_save_counter >= self.force_save_interval:
                self.save_current_position(force=True)
                self.force_save_counter = 0
            
            # 打印位置信息
            position_in_circles = software_position / self.pulses_per_rev
            self.get_logger().info(
                f'位置: {software_position}脉冲 ({position_in_circles:.2f}圈) '
                f'限位状态: {self.limit_status.name}',
                throttle_duration_sec=2.0
            )

    def reset_emergency_stop(self):
        """重置紧急停止状态"""
        self.emergency_stop_triggered = False
        self.get_logger().info('紧急停止状态已重置')

    def move_to_software_position(self, target_software_position, speed=200, acc_time=10):
        """移动到指定的软件位置"""
        can_move, reason = self.can_move_to_position(target_software_position)
        if not can_move:
            self.get_logger().error(f'无法移动到目标位置 {target_software_position}: {reason}')
            return False
        
        if self.emergency_stop_triggered:
            self.get_logger().error('紧急停止状态中，无法移动')
            return False
        
        try:
            with self.position_lock:
                position_diff = target_software_position - self.current_software_position
            
            self.get_logger().info(f'移动到软件位置: {target_software_position}, 增量: {position_diff}')
            
            self.is_moving = True
            self.current_move_target = target_software_position
            
            moving_msg = Bool()
            moving_msg.data = True
            self.moving_status_pub.publish(moving_msg)
            
            success = self.move_to_position(position_diff, speed, acc_time)
            
            self.is_moving = False
            moving_msg.data = False
            self.moving_status_pub.publish(moving_msg)
            
            if success:
                self.save_current_position(force=True)
            
            return success
            
        except Exception as e:
            self.get_logger().error(f'软件位置移动失败: {e}')
            self.is_moving = False
            return False

    def move_in_software_range(self, start_position, end_position, speed=200):
        """在指定的软件位置区间内移动"""
        if start_position < self.soft_limit_min or end_position > self.soft_limit_max:
            self.get_logger().error(f'移动范围超出软限位: [{self.soft_limit_min}, {self.soft_limit_max}]')
            return False
        
        if not self.move_to_software_position(start_position, speed):
            return False
        
        if not self.move_to_software_position(end_position, speed):
            return False
        
        return True

    # 原有的硬件控制函数保持不变
    def stop_motor(self):
        """停止电机"""
        self.get_logger().info("停止电机")
        try:
            with self.ser_lock:
                commands = [
                    [self.slave_id, 0x06, 0x03, 0x05, 0x00, 0x00],
                    [self.slave_id, 0x06, 0x06, 0x03, 0x00, 0x00],
                    [self.slave_id, 0x06, 0x03, 0x03, 0x00, 0x00],
                ]
                
                for cmd in commands:
                    data = bytearray(cmd)
                    crc = self.calculate_crc(data)
                    data.extend(crc)
                    self.ser.write(data)
                    time.sleep(0.1)
                    self.ser.read(20)
            
            self.get_logger().info("电机已停止")
        except Exception as e:
            self.get_logger().error(f"停止电机时出错: {e}")

    def setup_velocity_mode(self):
        """设置速度控制模式"""
        if self.current_mode is not None:
            self.stop_motor()
            time.sleep(0.2)
            
        self.get_logger().info("设置速度控制模式...")
        
        commands = [
            [self.slave_id, 0x06, 0x02, 0x00, 0x00, 0x00],
            [self.slave_id, 0x06, 0x06, 0x02, 0x00, 0x00],
            [self.slave_id, 0x06, 0x03, 0x02, 0x00, 0x01],
            [self.slave_id, 0x06, 0x03, 0x03, 0x00, 0x00],
        ]
        
        try:
            with self.ser_lock:
                for cmd in commands:
                    data = bytearray(cmd)
                    crc = self.calculate_crc(data)
                    data.extend(crc)
                    self.ser.write(data)
                    time.sleep(0.1)
                    self.ser.read(20)
            
            self.current_mode = "velocity"
            self.get_logger().info("速度控制模式设置完成")
        except Exception as e:
            self.get_logger().error(f"设置速度模式失败: {e}")

    def setup_position_mode(self):
        """设置位置控制模式"""
        if self.current_mode is not None:
            self.stop_motor()
            time.sleep(0.2)
            
        self.get_logger().info("设置位置控制模式...")
        
        commands = [
            [self.slave_id, 0x06, 0x02, 0x00, 0x00, 0x01],
            [self.slave_id, 0x06, 0x05, 0x00, 0x00, 0x02],
            [self.slave_id, 0x06, 0x03, 0x02, 0x00, 0x01],
            [self.slave_id, 0x06, 0x03, 0x03, 0x00, 0x00],
            [self.slave_id, 0x06, 0x03, 0x04, 0x00, 0x1C],
            [self.slave_id, 0x06, 0x03, 0x05, 0x00, 0x00],
            [self.slave_id, 0x06, 0x11, 0x00, 0x00, 0x00],
            [self.slave_id, 0x06, 0x11, 0x01, 0x00, 0x01],
            [self.slave_id, 0x06, 0x11, 0x04, 0x00, 0x00],
        ]
        
        try:
            with self.ser_lock:
                for cmd in commands:
                    data = bytearray(cmd)
                    crc = self.calculate_crc(data)
                    data.extend(crc)
                    self.ser.write(data)
                    time.sleep(0.1)
                    self.ser.read(20)
            
            self.current_mode = "position"
            self.get_logger().info("位置控制模式设置完成")
        except Exception as e:
            self.get_logger().error(f"设置位置模式失败: {e}")

    def move_to_position(self, position, speed=200, acc_time=10):
        """硬件位置控制函数"""
        position_int = int(position)
        speed_int = int(speed)
        acc_time_int = int(acc_time)
        
        self.get_logger().info(f"硬件移动: 位置={position_int}, 速度={speed_int}rpm")
        
        try:
            with self.ser_lock:
                if position_int < 0:
                    position_uint32 = (1 << 32) + position_int
                else:
                    position_uint32 = position_int
                
                pos_low = position_uint32 & 0xFFFF
                pos_high = (position_uint32 >> 16) & 0xFFFF
                
                pos_cmd = bytearray([
                    self.slave_id, 0x10, 0x11, 0x0C, 0x00, 0x02, 0x04,
                    (pos_low >> 8) & 0xFF, pos_low & 0xFF,
                    (pos_high >> 8) & 0xFF, pos_high & 0xFF
                ])
                crc = self.calculate_crc(pos_cmd)
                pos_cmd.extend(crc)
                self.ser.write(pos_cmd)
                time.sleep(0.1)
                self.ser.read(20)
                
                speed_cmd = bytearray([
                    self.slave_id, 0x06, 0x11, 0x0E, 
                    (speed_int >> 8) & 0xFF, speed_int & 0xFF
                ])
                crc = self.calculate_crc(speed_cmd)
                speed_cmd.extend(crc)
                self.ser.write(speed_cmd)
                time.sleep(0.1)
                self.ser.read(20)
                
                acc_cmd = bytearray([
                    self.slave_id, 0x06, 0x11, 0x0F,
                    (acc_time_int >> 8) & 0xFF, acc_time_int & 0xFF
                ])
                crc = self.calculate_crc(acc_cmd)
                acc_cmd.extend(crc)
                self.ser.write(acc_cmd)
                time.sleep(0.1)
                self.ser.read(20)
                
                enable_cmd = bytearray([self.slave_id, 0x06, 0x03, 0x03, 0x00, 0x01])
                crc = self.calculate_crc(enable_cmd)
                enable_cmd.extend(crc)
                self.ser.write(enable_cmd)
                time.sleep(0.1)
                self.ser.read(20)
            
            time.sleep(0.2)
            
            with self.ser_lock:
                start_cmd = bytearray([self.slave_id, 0x06, 0x03, 0x05, 0x00, 0x01])
                crc = self.calculate_crc(start_cmd)
                start_cmd.extend(crc)
                self.ser.write(start_cmd)
                time.sleep(0.1)
                self.ser.read(20)
            
            wait_time = abs(position_int) / max(speed_int, 1) * 0.1 + acc_time_int / 1000
            self.get_logger().info(f"等待移动完成... ({wait_time:.1f}秒)")
            time.sleep(wait_time)
            
            with self.ser_lock:
                stop_cmd = bytearray([self.slave_id, 0x06, 0x03, 0x05, 0x00, 0x00])
                crc = self.calculate_crc(stop_cmd)
                stop_cmd.extend(crc)
                self.ser.write(stop_cmd)
                time.sleep(0.1)
            
            self.get_logger().info("移动完成")
            return True
            
        except Exception as e:
            self.get_logger().error(f"硬件移动失败: {e}")
            return False

    def set_velocity(self, velocity):
        """速度控制函数"""
        velocity_int = int(velocity)
        
        current_status = self.check_limits()
        
        if velocity_int > 0 and current_status in [LimitStatus.AT_MAX_LIMIT, LimitStatus.EXCEEDED_MAX_LIMIT]:
            self.get_logger().error('已达正向限位，无法正向移动')
            return
        
        if velocity_int < 0 and current_status in [LimitStatus.AT_MIN_LIMIT, LimitStatus.EXCEEDED_MIN_LIMIT]:
            self.get_logger().error('已达负向限位，无法负向移动')
            return
        
        self.get_logger().info(f"设置速度: {velocity_int}rpm")
        
        try:
            with self.ser_lock:
                if velocity_int < 0:
                    velocity_value = (1 << 16) + velocity_int
                else:
                    velocity_value = velocity_int
                
                speed_cmd = bytearray([
                    self.slave_id, 0x06, 0x06, 0x03,
                    (velocity_value >> 8) & 0xFF, velocity_value & 0xFF
                ])
                crc = self.calculate_crc(speed_cmd)
                speed_cmd.extend(crc)
                self.ser.write(speed_cmd)
                time.sleep(0.1)
                self.ser.read(20)
                
                enable_cmd = bytearray([self.slave_id, 0x06, 0x03, 0x03, 0x00, 0x01])
                crc = self.calculate_crc(enable_cmd)
                enable_cmd.extend(crc)
                self.ser.write(enable_cmd)
                time.sleep(0.1)
                self.ser.read(20)
            
            self.get_logger().info("速度设置完成")
        except Exception as e:
            self.get_logger().error(f"设置速度失败: {e}")

    # 回调函数
    def position_callback(self, msg):
        """位置控制回调"""
        target_position = float(msg.data)
        
        self.get_logger().info(f"接收软件位置命令: {target_position}")
        
        if self.current_mode != "position":
            self.setup_position_mode()
        self.move_to_software_position(target_position)
    
    def velocity_callback(self, msg):
        """速度控制回调"""
        velocity = float(msg.data)
        velocity_int = int(velocity)
        
        self.get_logger().info(f"接收速度命令: {msg.data} -> {velocity_int}")
        
        if self.current_mode != "velocity":
            self.setup_velocity_mode()
        self.set_velocity(velocity_int)
    
    def circle_callback(self, msg):
        """圈数控制回调"""
        circles = float(msg.data)
        target_position = circles * self.pulses_per_rev
        
        self.get_logger().info(f"接收圈数命令: {circles}圈 -> 位置 {target_position}")
        
        if self.current_mode != "position":
            self.setup_position_mode()
        self.move_to_software_position(target_position)
    
    def control_mode_callback(self, msg):
        """控制模式切换回调"""
        mode = msg.data
        if mode == 0:
            self.setup_velocity_mode()
        elif mode == 1:
            self.setup_position_mode()
        else:
            self.get_logger().warn(f'未知的控制模式: {mode}')
    
    def emergency_stop_callback(self, msg):
        """紧急停止回调"""
        if msg.data:
            self.emergency_stop()
        else:
            self.reset_emergency_stop()
    
    def destroy_node(self):
        """节点销毁时的清理工作"""
        self.get_logger().info("保存最终位置...")
        self.emergency_save()
        
        if self.ser and self.ser.is_open:
            self.stop_motor()
            self.ser.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    
    node = AIMotorControl()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("接收到Ctrl+C，正在关闭节点...")
    except Exception as e:
        node.get_logger().error(f"节点运行异常: {e}")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()